#include "matrix4x4.h"

#include <cmath>

float Matrix4x4::operator()(int nLine, int nColumn)
{
	return fvMatrix[nLine][nColumn];
}

Matrix4x4* Matrix4x4::operator*=(Matrix4x4 mat)
{
	float fvMul[4][4];

	for(int i = 0; i < 4; ++i)
	{
	    for(int j = 0; j < 4; ++j)
        {
            fvMul[i][j]=0;
            for(int k = 0; k < 4; ++k)
            {
                fvMul[i][j] += fvMatrix[i][k] * mat(k, j);
            }
        }
	}

	for(int i = 0; i < 4; ++i)
	{
		for(int j = 0; j < 4; ++j)
		{
			fvMatrix[i][j] = fvMul[i][j];
		}
	}
	return this;
}

Matrix4x4* Matrix4x4::operator*=(float fvMul[16])
{
    float fvNew[4][4];

	for(int i = 0; i < 4; ++i)
	{
	    for(int j = 0; j < 4; ++j)
        {
            fvNew[i][j]=0;
            for(int k = 0; k < 4; ++k)
            {
                fvNew[i][j] += fvMatrix[i][k] * fvMul[k*4+j];
            }
        }
	}

	for(int i = 0; i < 4; ++i)
	{
		for(int j = 0; j < 4; ++j)
		{
			fvMatrix[i][j] = fvNew[i][j];
		}
	}
	return this;
}

Matrix4x4* Matrix4x4::operator*=(float fvMul[4][4])
{
    float fvNew[4][4];

	for(int i = 0; i < 4; ++i)
	{
	    for(int j = 0; j < 4; ++j)
        {
            fvNew[i][j]=0;
            for(int k = 0; k < 4; ++k)
            {
                fvNew[i][j] += fvMatrix[i][k] * fvMul[k][j];
            }
        }
	}

	for(int i = 0; i < 4; ++i)
	{
		for(int j = 0; j < 4; ++j)
		{
			fvMatrix[i][j] = fvNew[i][j];
		}
	}
	return this;
}

Matrix4x4* Matrix4x4::Projection(float fov, float pw, float ph, float front, float back)
{
    float xymax = front * tan(fov * 3.14f);
    float ymin = -xymax;
    float xmin = -xymax;

    float width = xymax - xmin;
    float height = xymax - ymin;

    float depth = back - front;
    float q = -(back + front) / depth;
    float qn = -2 * (back * front) / depth;

    float w = 2 * front / width;
    w = w / (pw/ph);
    float h = 2 * front / height;

    fvMatrix[0][0] = w;
    fvMatrix[0][1] = 0;
    fvMatrix[0][2] = 0;
    fvMatrix[0][3] = 0;

    fvMatrix[1][0] = 0;
    fvMatrix[1][1] = h;
    fvMatrix[1][2] = 0;
    fvMatrix[1][3] = 0;

    fvMatrix[2][0] = 0;
    fvMatrix[2][1] = 0;
    fvMatrix[2][2] = q;
    fvMatrix[2][3] = -1;

    fvMatrix[3][0] = 0;
    fvMatrix[3][1] = 0;
    fvMatrix[3][2] = qn;
    fvMatrix[3][3] = 0;

    return this;
}

Matrix4x4* Matrix4x4::Identity()
{
    for(int i = 0; i < 4; ++i)
    {
        for(int j = 0; j < 4; ++j)
        {
            if(i == j) fvMatrix[i][j] = 1;
            else fvMatrix[i][j] = 0;
        }
    }

    return this;
}

float* Matrix4x4::GetTab()
{
    float *tab = new float[16];
    for(int i = 0; i < 4; ++i)
    {
        for(int j = 0; j < 4; ++j)
        {
            tab[i*4+j] = fvMatrix[i][j]; //NOT SURE, TODO: CHECK IT
        }
    }
    return tab;
}

void Matrix4x4::Set(int nLine, int nColumn, float fValue)
{
    fvMatrix[nLine][nColumn] = fValue;
}

void Matrix4x4::Set(float fvNewMatrix[16])
{
    for(int i = 0; i < 4; ++i)
    {
        for(int j = 0; j < 4; ++j)
        {
            fvMatrix[i][j] = fvNewMatrix[i*4+j];
        }
    }
}

void Matrix4x4::Set(float fvNewMatrix[4][4])
{
    for(int i = 0; i < 4; ++i)
    {
        for(int j = 0; j < 4; ++j)
        {
            fvMatrix[i][j] = fvNewMatrix[i][j];
        }
    }
}

Matrix4x4* Matrix4x4::Translate(float x, float y, float z)
{
    Matrix4x4 matTranslate;

    matTranslate.Identity();

    matTranslate.Set(0, 3, x);
    matTranslate.Set(1, 3, y);
    matTranslate.Set(2, 3, z);

    *this *= matTranslate;

    return this;
}

Matrix4x4* Matrix4x4::Translate(Vector3 vecTranslation)
{
    return Translate(vecTranslation.x, vecTranslation.y, vecTranslation.z);
}

Matrix4x4* Matrix4x4::Rotate(float x, float y, float z)
{
    if(x == 0.0f && y == 0.0f && z == 0.0f) return this;

    if(x != 0.0f)
    {
        float fvRot[16] = {0};
        fvRot[0] = fvRot[5] = fvRot[10] = fvRot[15] = 1.0f; //identity :>

        x *= PI/180.0f;

        fvRot[5] = cos(x);
        fvRot[6] = -sin(x);
        fvRot[9] = sin(x);
        fvRot[10] = cos(x);

        *this *= fvRot;
    }
    if(y != 0.0f)
    {
        float fvRot[16] = {0};
        fvRot[0] = fvRot[5] = fvRot[10] = fvRot[15] = 1.0f; //identity :>

        y *= PI/180.0f;

        fvRot[0] = cos(y);
        fvRot[2] = sin(y);
        fvRot[8] = -sin(y);
        fvRot[10] = cos(y);

        *this *= fvRot;
    }
    if(z != 0.0f)
    {
        float fvRot[16] = {0};
        fvRot[0] = fvRot[5] = fvRot[10] = fvRot[15] = 1.0f; //identity :>

        z *= PI/180.0f;

        fvRot[0] = cos(z);
        fvRot[1] = -sin(z);
        fvRot[4] = sin(z);
        fvRot[5] = cos(z);

        *this *= fvRot;
    }

    return this;
}

Matrix4x4* Matrix4x4::Scale(float x, float y, float z)
{
    float fvScale[16];
    fvScale[0] = fvScale[5] = fvScale[10] = fvScale[15] = 1.0f;

    fvScale[0] = x;
    fvScale[5] = y;
    fvScale[10] = z;

    *this *= fvScale;

    return this;
}

Matrix4x4* Matrix4x4::Look(float x, float y, float z, float atX, float atY, float atZ)
{
    Identity();

    Rotate(-ToDeg(atan((atY-y)/(atZ-z))), ToDeg(atan((atX-x)/(atZ-z))), 180.0f);
    Translate(-x, -y, -z);

    return this;
}
